//
// Created by An on 2025/7/7.
//

#ifndef LIGHTCAR2025_MOTOR_H
#define LIGHTCAR2025_MOTOR_H

#include "tim.h"
#include "PID.h"


typedef struct {
  // === 编码器 ===
  TIM_HandleTypeDef *htim_encoder;
  uint16_t last_encoder_value;
  int32_t encoder_count;

  // === PWM控制 ===
  TIM_HandleTypeDef *htim_pwm;
  uint32_t pwm_channel;
  GPIO_TypeDef *dir_port1;
  uint16_t dir_pin1;
  GPIO_TypeDef *dir_port2;
  uint16_t dir_pin2;

  // === 控制目标 ===
  int32_t target_position;  // mm
  int32_t target_speed;     // mm/s
  int32_t current_speed;    // mm/s

  // === 双环控制器 ===
  PID_t speed_pid;
  PID_t position_pid;

} Motor_t;



extern Motor_t motor_A;  // Front Left
extern Motor_t motor_B;  // Front Right
extern Motor_t motor_C;  // Back Left
extern Motor_t motor_D;  // Back Right

void Motors_Init(void);   // 初始化所有电机
void Motor_SetPWM(Motor_t *motor, int32_t pwm_val);
void Motor_UpdateEncoder(Motor_t *motor);
void Motor_EstimateSpeed(Motor_t *motor, float delta_time_s);
float Motor_GetPosition(Motor_t *motor);

void Motor_SpeedControl(Motor_t *motor);
void Motor_PositionControl(Motor_t *motor);



#endif //LIGHTCAR2025_MOTOR_H
